l1 = 0.26;
l2 = 0.2695;
l3 = 0.2615;
z1 = 0.4-0.107-l3*cos(pi/2)
z2 = sqrt(0.4^2+0.4^2)-l3*sin(pi/2)

2*atan((2*l1*z2 + (- l1^4 + 2*l1^2*l2^2 + 2*l1^2*z1^2 + 2*l1^2*z2^2 - l2^4 + 2*l2^2*z1^2 + 2*l2^2*z2^2 - z1^4 - 2*z1^2*z2^2 - z2^4)^(1/2))/(l1^2 + 2*l1*z1 - l2^2 + z1^2 + z2^2))
2*atan((2*l1*z2 - (- l1^4 + 2*l1^2*l2^2 + 2*l1^2*z1^2 + 2*l1^2*z2^2 - l2^4 + 2*l2^2*z1^2 + 2*l2^2*z2^2 - z1^4 - 2*z1^2*z2^2 - z2^4)^(1/2))/(l1^2 + 2*l1*z1 - l2^2 + z1^2 + z2^2))

syms tx ty tz;
Rz = [cos(tz) -sin(tz) 0; sin(tz) cos(tz) 0; 0 0 1];
Ry = [cos(ty) 0 sin(ty); 0 1 0; -sin(ty) 0 cos(ty)];
Rx = [1 0 0; 0 cos(tx) -sin(tx); 0 sin(tx) cos(tx)];
R = Rx*Ry*Rz
R_inverse = Rz*Ry*Rx